#include <lversion.h>
#if L_VERSION_MODULE_COLLISION_DETECTOR == 120104

#include "_lcollisionDetector_120104.h"

#include <ltriangle.h>
using namespace llib;
using namespace llib::collision;
using namespace llib::geom;

#include <deque>
using namespace std;

/* In this CD algorithm, we're actually using Quad-tree BVTT.
 * We test all internal node in same depth, expect reach the leaf node.
 * And what's more, it's a nonrecurrent one.
 */
void CollisionDetector::CollisionDetect( BVNode* node0, BVNode* node1, CollisionResult* cr )
{
    typedef pair<BVNode*,BVNode*> NodePair;
    deque<NodePair> _stack;
    _stack.push_back(NodePair(node0, node1));
    NodePair nodePair;
    BVNode *lchild0, *rchild0;
    BVNode *lchild1, *rchild1;
    MeshFace leaf0, leaf1;

    while(!_stack.empty()) {
        nodePair = _stack.back();
        _stack.pop_back();
        node0 = nodePair.first;
        node1 = nodePair.second;

        if(node0 == NULL || node1 == NULL)
            continue; // invalid test pair

        if(BVNode::CollisionCulling(node0, node1))
            continue; // no collision between node pair, culled.

        lchild0 = node0->GetChildLeft();
        rchild0 = node0->GetChildRight();
        lchild1 = node1->GetChildLeft();
        rchild1 = node1->GetChildRight();
        if(!node0->IsLeaf()) {
            if(!node1->IsLeaf()) {
                _stack.push_back(NodePair(lchild0,lchild1));
                _stack.push_back(NodePair(lchild0,rchild1));
                _stack.push_back(NodePair(rchild0,lchild1));
                _stack.push_back(NodePair(rchild0,rchild1));
            } else {
                _stack.push_back(NodePair(lchild0,node1));
                _stack.push_back(NodePair(rchild0,node1));
            }
        } else {
            if(!node1->IsLeaf()) {
                _stack.push_back(NodePair(node0,lchild1));
                _stack.push_back(NodePair(node0,rchild1));
            } else {
                // the only and last case which needs to process primitive test
                leaf0 = node0->GetLeafMeshFace();
                leaf1 = node1->GetLeafMeshFace();
                // check neighbor first
                if( MeshFace::CheckNeighbour(leaf0, leaf1) )
                    continue;
                // here goes to the test
                if( TriPairTester::TestStatic(leaf0.GetTriangle(),leaf1.GetTriangle()) ) {
                    cr->AddPair(leaf0, leaf1);
                }
            }
        }
    }
}

/* The self collision detection algorithm
 * Also it's nonrecurrent
 */
void CollisionDetector::SelfCollisionDetect( BVNode* node, CollisionResult* cr )
{
    deque<BVNode*> _stack;
    _stack.push_back(node);
    BVNode *lchild, *rchild;

    while(!_stack.empty()) {
        node = _stack.back();
        _stack.pop_back();

        if(node == NULL) continue; // no need to test

        if(BVNode::SelfCollisionCulling(node))
            continue; //  no self collision

        lchild = node->GetChildLeft();
        rchild = node->GetChildLeft();
        if(lchild != NULL) {
            _stack.push_back(lchild);
            if(rchild != NULL) {
                _stack.push_back(rchild);
                // check collision between childs
                CollisionDetect(lchild, rchild, cr);
            }
        } else if(rchild != NULL) {
            _stack.push_back(rchild);
        }
    }
}

#endif
